#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
四足机器人轨迹规划器模块
包含轨迹规划器类，负责生成机器人腿部轨迹点
"""

import numpy as np
from math import pi, cos, sin
from config import LEG_PARAMS

class TrajectoryPlanner:
    """轨迹规划器类，生成轨迹点"""
    
    def __init__(self):
        """初始化轨迹规划器"""
        # 当前参数 (深拷贝默认参数)
        self.current_params = {}
        for leg_id, params in LEG_PARAMS.items():
            self.current_params[leg_id] = params.copy()
    
    def generate_trajectory(self, leg_id, time_phase):
        """生成轨迹点
        参数:
            leg_id: 腿的ID
            time_phase: 时间相位 (0-1)
        返回:
            target_pos: 目标位置 (x, y, z)
        """
        params = self.current_params[leg_id]
        status = 0
        # 计算腿部相位
        leg_phase = (time_phase  + params["phase_offset"]) % 1.0
        # 计算椭圆轨迹点
        if 0 <= leg_phase < 0.3:  # 摆动相
            swing_progress = leg_phase / 0.3
            angle = pi - swing_progress * pi
            status = 1  # 摆动相状态
            # 使用固定方向 "Y"
            x_offset = params["short_axis"] * sin(angle) if params["short_axis"] > 0 else 0
            y_offset = params["long_axis"] * cos(angle)
            z_offset = params["height"] * sin(angle)
            # 打印leg_id
            # if leg_id == "LF":  # 左前腿
            #     print(f"leg_id = {leg_id}, 相位 = {time_phase:.3f}, 动作 = 摆动")


        else:  # 支撑相
            return_progress = (leg_phase - 0.3) / 0.7
            # 使用固定方向 "Y"
            x_offset = 0

            y_offset = (params["long_axis"] - 2 * params["long_axis"] * return_progress)
            z_offset = 0
            status = 0
            # if leg_id == "LF":  # 左前腿
            #     print(f"leg_id = {leg_id}, 相位 = {time_phase:.3f}, 动作 = 落地")

        # 计算最终位置
        x = params["center_x"] + x_offset
        y = params["center_y"] + y_offset
        z = params["base_z"] + z_offset
        
        return np.array([x, y, z]),status
